#ifndef MOVE_YOUBOT_BASE_SERVER_H_
#define MOVE_YOUBOT_BASE_SERVER_H_

#include <iostream>
#include <assert.h>
#include <vector>

#include <boost/units/systems/si/velocity.hpp>
#include <boost/units/systems/si/angular_velocity.hpp>
#include <boost/units/systems/si/io.hpp>

#include "ros/ros.h"
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Twist.h>

#include <tf/transform_listener.h>

#include <actionlib/server/simple_action_server.h>

#include "youbot_relay/MoveYoubotBaseAction.h"

//#include <boost/units/io.hpp>
//#include <boost/units/systems/angle/degrees.hpp>
//#include <boost/units/conversion.hpp>

namespace move_youbot_base
{

/** A class that uses the actionlib::ActionServer interface that moves the youbot base to a goal location.            */
class MoveYoubotBaseServer
{
public:

  MoveYoubotBaseServer(ros::NodeHandle n, tf::TransformListener& tf);

  virtual ~MoveYoubotBaseServer();

private:

  void execute(const youbot_relay::MoveYoubotBaseGoalConstPtr& move_youbot_base_goal);

  /** @brief      Checks if the quaternion is a valid navigation goal.
   *  @details    Valid quaternion should have non-zero length and be close to vertical.
   *  @param[in]  q Quaternion to be checked.
   *  @return     true if the quaternion is a valid navigation goal, false otherwise.                                 */
  bool isQuaternionValid(const geometry_msgs::Quaternion& q);

  /** @brief      Set a new goal pose for youbot base.
   *  @details    Checks if the new pose is valid and converts it to the global frame.
   *  @param[in]  new_goal New goal (as received by action server).
   *  @return     true if the new goal is valid and was accepted, false otherwise.                                    */
  bool setNewGoal(const youbot_relay::MoveYoubotBaseGoalConstPtr& new_goal);

  /** @brief      Command velocities to move towards the current goal and check if the goal was reached.
   *  @details    This function is supposed to be called regularly in the action execution loop.
   *  @return     true if the goal was reached, false otherwise.                                                      */
  bool moveTowardsGoal();

  /** @brief     Publishes a velocity command for the youbot base.                                                    */
  void publishVelocity(double x, double y, double z);

  /** @brief     Publishes a velocity command with zero speed to the youbot base.                                     */
  void publishZeroVelocity();

  /** @brief     Calculate the two-dimensional euclidean distance between two points.                                 */
  static double calculateDistance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2);

  /** @brief     Calculate a value inside a range which corresponds to a certain percent of the range.                */
  static double percentageOfRange(double min, double max, int percentage);

  geometry_msgs::PoseStamped transformPose(const geometry_msgs::PoseStamped& pose_msg,
                                           const std::string& source_frame_id, const std::string& target_frame_id);

  typedef actionlib::SimpleActionServer<youbot_relay::MoveYoubotBaseAction> MoveYoubotBaseActionServer;

  /// Action server
  MoveYoubotBaseActionServer* action_server_;

  /// Publisher of velocity commands to the youbot base.
  ros::Publisher velocity_publisher_;

  /// ROS node handle.
  ros::NodeHandle node_;

  /// Reference to a transform listener.
  tf::TransformListener& transform_listener_;

  /// Frequency at which the velocity commands to youbot base are reissued during action execution.
  double controller_frequency_;

  /// Current goal pose in global reference frame.
  geometry_msgs::PoseStamped global_goal_;

  std::string global_frame_id_;
  std::string youbot_base_frame_id_;

  //tf::Stamped<tf::Pose> global_pose_;
  //ros::Publisher current_goal_pub_, action_goal_pub_;
  //ros::Subscriber goal_sub_;
  //ros::ServiceServer make_plan_srv_, clear_unknown_srv_, clear_costmaps_srv_;
  //MoveBaseState state_;
  //ros::Time last_valid_plan_, last_valid_control_, last_oscillation_reset_;
  //geometry_msgs::PoseStamped oscillation_pose_;

  /// Maximum allowed velocity for translational movements (m/s)
  double max_translational_velocity_;

  /// Minimum allowed velocity for translational movements (m/s)
  double min_translational_velocity_;

  /// Maximum allowed velocity for rotational movements (rad/s)
  double max_rotational_velocity_;

  /// Minimum allowed velocity for rotational movements (rad/s)
  double min_rotational_velocity_;

  /// The velocity of translational movement to be used to achieve the current goal (m/s)
  double translational_velocity_;

  /// The velocity of rotational movement to be used to achieve the current goal (m/s)
  double rotational_velocity_;

  static const double translational_tolerance_ = 0.01; // meters TODO: turn it into a node parameter
  static const double rotational_tolerance_ = 0.01; // radians TODO: turn it into a node parameter
};

}

#endif /* MOVE_YOUBOT_BASE_SERVER_H_ */

